#include "user.h"
#include "include.h"
#include "Ano_RC.h"
#include "Ano_ProgramCtrl_User.h"
#include "Ano_OPMV_CBTracking_Ctrl.h"

void RC_Ctrl_take_off(uint16_t dT_ms)
{
    static uint16_t dT_cnt = 0;
    static uint8_t is_taken_off = 0;

    if (switchs.t265_on && flag.flight_mode == LOC_HOLD) {
        if (CH_N[AUX2] > 100 && is_taken_off == 0)
        {
            flag.unlock_cmd = 1;

            dT_cnt += dT_ms;

            if (dT_cnt == 2000) {
                flag.taking_off = 1;

                is_taken_off = 1;
                dT_cnt = 0;
            }
        } else if (CH_N[AUX2] < -100 && is_taken_off == 1)
        {
            Program_Ctrl_User_Set_HXYcmps(0, 0);

            flag.auto_take_off_land = AUTO_LAND;

            is_taken_off = 0;
        }
    }
}

uint8_t user_omv_on = 0;

void RC_OMV_Ctrl(uint16_t dT_ms)
{
    static uint16_t dT_cnt = 0;
    static uint16_t dT_omv_cnt = 0;

    if (CH_N[AUX3] > 100 && flag.flying == 1 && user_omv_on == 0) {
        if (dT_cnt < 3000) {
            dT_cnt += dT_ms;

            Program_Ctrl_User_Set_HXYcmps(20, 0);
        } else if (user_omv_on == 0) {
            user_omv_on = 1;
            dT_cnt = 0;

            ANO_DT_SendString("OMV ON");
        }
    } else if(CH_N[AUX3] < -100){
        Program_Ctrl_User_Set_HXYcmps(0, 0);
        Program_Ctrl_User_Set_YAWdps(0);
        user_omv_on = 0;
        dT_cnt = 0;
        dT_omv_cnt = 0;
    }

    if (ano_opmv_cbt_ctrl.target_loss == 0) {
        if (dT_omv_cnt > 3000) {
            user_omv_on = 0;

            Program_Ctrl_User_Set_HXYcmps(0, 0);

            flag.auto_take_off_land = AUTO_LAND;
        }else{
            dT_omv_cnt += dT_ms;
        }
    }else{
        dT_omv_cnt = 0;
    }
}

//void RC_Ctrl_auto_move(uint16_t dT_ms)
//{
//    static uint16_t dT_cnt = 0;
//
//    if (CH_N[AUX3] > 100 && flag.flying == 1)
//    {
//        if (dT_cnt < 8000)
//        {
//            dT_cnt += dT_ms;
//        }
//
//        if (dT_cnt <= 2000)
//        {
//            Program_Ctrl_User_Set_HXYcmps(50, 0);
//        }
//        else if (dT_cnt > 2000 && dT_cnt <= 4000)
//        {
//            Program_Ctrl_User_Set_HXYcmps(0, 0);
//            Program_Ctrl_User_Set_YAWdps(45);
//        }
//        else if (dT_cnt > 4000 && dT_cnt <= 6000)
//        {
//            Program_Ctrl_User_Set_YAWdps(0);
//            Program_Ctrl_User_Set_HXYcmps(20, 0);
//        }
//        else
//        {
//            Program_Ctrl_User_Set_YAWdps(0);
//            Program_Ctrl_User_Set_HXYcmps(0, 0);
//
//            dT_cnt = 8000;
//        }
//    }
//    else
//    {
//        Program_Ctrl_User_Set_YAWdps(0);
//        Program_Ctrl_User_Set_HXYcmps(0, 0);
//    }
//}
